import time
  
class LX225BusServo:
  def __init__(self,uart):
    """
    uart:UART(波特率115200)
    """
    
    self.uart = uart

  def check_sum(self, data):
    """
    计算校验和
    """
    check_sum = 0
    for i in data:
        check_sum += i
    # 取最低的一个字节
    check_sum = check_sum & 0xFF
    # 取反
    check_sum = ~check_sum & 0xFF
    return check_sum
  
  def send_command(self, data):
    """
    发送指令
    """
    # 指令头
    header = [0x55, 0x55]
    # 校验和
    check_sum = self.check_sum(data)
    # 发送数据
    print("[发送原始数据] " + ', '.join(f'0x{b:02x}' for b in header + data + [check_sum]))
    self.uart.write(bytearray(header + data + [check_sum]))
    #self.uart.readall()#清空缓冲区
    # 等待回应
    time.sleep(0.01)
    response = self.uart.readall()
    return response
  
  def get_servo_id(self, id=0xfe):
    """
    获取指定舵机ID(舵机是否存在)0xfe:查询所有舵机
    id:舵机ID
    """
    data = [id,0x03,0x0e]
    response = self.send_command(data)
    return response
  
  def rename_servo(self, id=0xfe, new_id = 0x00):
    """
    重命名舵机ID
    id:舵机ID
    new_id:新舵机ID
    """
    data = [id,0x04,0x0d,new_id]
    response = self.send_command(data)
    return response
  
  def set_angle(self, id=0xfe, angle=500, time=500):
    """
    设置舵机角度
    id:舵机ID
    angle:角度值(推荐0-1000)
    time:转动时间(推荐0-30000)
    """
    angle_low = angle & 0xFF
    angle_high = (angle >> 8) & 0xFF
    time_low = time & 0xFF
    time_high = (time >> 8) & 0xFF
    data = [id,0x07,0x01,angle_low,angle_high,time_low,time_high]
    response = self.send_command(data)
    return response
  
  def set_mission_angle(self, id=0xfe, angle=500, time=500):
    """
    设置舵机角度(与set_angle相似,但需要mission_launch来激活)
    id:舵机ID
    angle:角度值(推荐0-1000)
    time:转动时间(推荐0-30000)
    """
    angle_low = angle & 0xFF
    angle_high = (angle >> 8) & 0xFF
    time_low = time & 0xFF
    time_high = (time >> 8) & 0xFF
    data = [id,0x07,0x07,angle_low,angle_high,time_low,time_high]
    response = self.send_command(data)
    return response
  
  def mission_launch(self, id=0xfe):
    """
    启动舵机转动
    id:舵机ID
    """
    data = [id,0x03,0x0b]
    response = self.send_command(data)
    return response
  
  def get_vin(self,id=0xfe):
    """
    获取舵机电压
    id:舵机ID
    """
    data = [id,0x03,0x1b]
    response = self.send_command(data)
    return response
  
  def get_temp(self,id=0xfe):
    """
    获取舵机温度
    id:舵机ID
    """
    data = [id,0x03,0x1a]
    response = self.send_command(data)
    return response
  
  def servo_off(self,id=0xfe):
    """
    关闭舵机
    id:舵机ID
    """
    data = [id,0x04,0x1f,0x00]
    response = self.send_command(data)
    return response
  def servo_on(self,id=0xfe):
    """
    开启舵机
    id:舵机ID
    """
    data = [id,0x04,0x1f,0x01]
    response = self.send_command(data)
    return response
  
  def read_angle(self,id=0xfe):
    """
    读取舵机角度
    id:舵机ID
    """
    data = [id,0x03,0x1c]
    response = self.send_command(data)
    return response



















